/*
Copyright 2007 Luke Benstead

This file is part of slMath.

slMath is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser Public License as published by
the Free Software Foundation; either version 3 of the License, or
(at your option) any later version.

slMath is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser Public License for more details.

You should have received a copy of the GNU Lesser Public License
along with this program.  If not, see <http://www.gnu.org/licenses/>.

*/

#ifdef __CPLUSPLUS
	#include <memory>
	#include <cassert>
#else
	#include <memory.h>
	#include <assert.h>
#endif

#include "math/utility.h"
#include "math/vec4.h"
#include "math/mat4.h"

/// Adds 2 4D vectors together. The result is store in pOut, the function returns
/// pOut so that it can be nested in another function.
slVec4* slVec4Add(slVec4* pOut, const slVec4* pV1, const slVec4* pV2)
{
	pOut->m_X = pV1->m_X - pV2->m_X;
	pOut->m_Y = pV1->m_Y - pV2->m_Y;
	pOut->m_Z = pV1->m_Z - pV2->m_Z;
	pOut->m_W = pV1->m_W - pV2->m_W;

	return pOut;
}

/*
slVec4* slVec4Cross(slVec4* pOut, const slVec4* pV1, const slVec4* pV2, const slVec4* pV3)
{
	pOut->m_X = pV1->m_Y * (pV2->m_Z * pV3->m_W - pV3->m_Z * pV2->m_W) - pV1->m_Z * (pV2->
	pOut->m_Y =
	pOut->m_Z =
	pOut->m_W =


| i  j  k  l  |   | ay*(bz*cw - cz*bw) - az*(by*cw - cy*bw) + aw*(by*cz - cy*bz) |
| ax ay az aw |   |-ax*(bz*cw - cz*bw) + az*(bx*cw - cx*bw) - aw*(bx*cz - cx*bz) |
| bx by bz bw | = | ax*(by*cw - cy*bw) - ay*(bx*cw - cx*bw) + aw*(bx*cy - cx*by) |
| cx cy cz cw |   |-ax*(by*cz - cy*bz) + ay*(bx*cz - cx*bz) - az*(bx*cy - cx*by) |

	return pOut;
}*/

/// Returns the dot product of 2 4D vectors
slScalar slVec4Dot(const slVec4* pV1, const slVec4* pV2)
{
	return (  pV1->m_X * pV2->m_X
			+ pV1->m_Y * pV2->m_Y
			+ pV1->m_Z * pV2->m_Z
			+ pV1->m_W * pV2->m_W );
}

/// Returns the length of a 4D vector, this uses a sqrt so if the squared length will do use
/// slVec4LengthSq
slScalar slVec4Length(const slVec4* pIn)
{
	return slSqrt(slSQR(pIn->m_X) + slSQR(pIn->m_Y) + slSQR(pIn->m_Z) + slSQR(pIn->m_W));
}

/// Returns the length of the 4D vector squared.
slScalar slVec4LengthSq(const slVec4* pIn)
{
	return slSQR(pIn->m_X) + slSQR(pIn->m_Y) + slSQR(pIn->m_Z) + slSQR(pIn->m_W);
}

/// Returns the interpolation of 2 4D vectors based on t. Currently not implemented!
slVec4* slVec4Lerp(slVec4* pOut, const slVec4* pV1, const slVec4* pV2, slScalar t)
{
    assert(0);
    return pOut;
}

/// Normalizes a 4D vector. The result is stored in pOut. pOut is returned
slVec4* slVec4Normalize(slVec4* pOut, const slVec4* pIn)
{
	slScalar l = 1.0 / slVec4Length(pIn);

	pOut->m_X *= l;
	pOut->m_Y *= l;
	pOut->m_Z *= l;
	pOut->m_W *= l;

	return pOut;
}

/// Scales a vector to the required length. This performs a Normalize before multiplying by S.
slVec4* slVec4Scale(slVec4* pOut, const slVec4* pIn, const slScalar s)
{
	slVec4Normalize(pOut, pIn);

	pOut->m_X *= s;
	pOut->m_Y *= s;
	pOut->m_Z *= s;
	pOut->m_W *= s;
	return pOut;
}

/// Subtracts one 4D pV2 from pV1. The result is stored in pOut. pOut is returned
slVec4* slVec4Subtract(slVec4* pOut, const slVec4* pV1, const slVec4* pV2)
{
	pOut->m_X = pV1->m_X - pV2->m_X;
	pOut->m_Y = pV1->m_Y - pV2->m_Y;
	pOut->m_Z = pV1->m_Z - pV2->m_Z;
	pOut->m_W = pV1->m_W - pV2->m_W;

	return pOut;
}

/// Transforms a 4D vector by a matrix, the result is stored in pOut, and pOut is returned.
slVec4* slVec4Transform(slVec4* pOut, const slVec4* pV, const slMat4* pM)
{
	pOut->m_X = pV->m_X * pM->m_Mat[0] + pV->m_Y * pM->m_Mat[4] + pV->m_Z * pM->m_Mat[8] + pV->m_W * pM->m_Mat[12];
	pOut->m_Y = pV->m_X * pM->m_Mat[1] + pV->m_Y * pM->m_Mat[5] + pV->m_Z * pM->m_Mat[9] + pV->m_W * pM->m_Mat[13];
	pOut->m_Z = pV->m_X * pM->m_Mat[2] + pV->m_Y * pM->m_Mat[6] + pV->m_Z * pM->m_Mat[10] + pV->m_W * pM->m_Mat[14];
    pOut->m_W = pV->m_X * pM->m_Mat[3] + pV->m_Y * pM->m_Mat[7] + pV->m_Z * pM->m_Mat[11] + pV->m_W * pM->m_Mat[15];
	return pOut;
}

/// Loops through an input array transforming each vec4 by the matrix.
slVec4* slVec4TransformArray(slVec4* pOut, unsigned int outStride,
			const slVec4* pV, unsigned int vStride, const slMat4* pM, unsigned int count)
{
    unsigned int i = 0;
    while (i < count) //Go through all of the vectors
    {
        const slVec4* in = pV + (i * vStride); //Get a pointer to the current input
        slVec4* out = pOut + (i * outStride); //and the current output

        slVec4Transform(out, in, pM); //Perform transform on it

        ++i;
    }

    return pOut;
}

bool 	slVec4AreEqual(const slVec4* p1, const slVec4* p2)
{
	return (
				(p1->m_X < p2->m_X + slEpsilon && p1->m_X > p2->m_X - slEpsilon) &&
				(p1->m_Y < p2->m_Y + slEpsilon && p1->m_Y > p2->m_Y - slEpsilon) &&
				(p1->m_Z < p2->m_Z + slEpsilon && p1->m_Z > p2->m_Z - slEpsilon) &&
				(p1->m_W < p2->m_W + slEpsilon && p1->m_W > p2->m_W - slEpsilon)
			);
}

slVec4* slVec4Assign(slVec4* pOut, const slVec4* pIn)
{
	assert(pOut != pIn);
	
	memcpy(pOut, pIn, sizeof(float) * 4);
	
	return pOut;
}

